#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <iostream>
#include <stdio.h>
#include <math.h>

using namespace std;
using namespace cv;

#ifndef EPISLON
#define EPISLON 0.0001
#endif

void HandDetect(Mat rgbImg, Mat depthImg, Rect face, Rect upperbody);

void detectAndDraw(Mat& img, Mat& depthimg,
                   CascadeClassifier& cascade, CascadeClassifier& nestedCascade,
                   double scale);

bool CheckFaceDepth(int depth, int size)
{
    bool flag = false;
    double idealDepth = 0.0524 * size * size - 7.2307 * size + 286.0086;

    if ((depth > idealDepth - 5) && (depth < idealDepth + 5))
        return true;
    else
        return false;
}

bool CheckUpperbodyDepth(int depth, int size)
{
    bool flag = false;
    double idealDepth = 0.001 * size * size - 0.7095 * size + 130.9962;

    if ((depth > idealDepth - 8) && (depth < idealDepth + 8))
        return true;
    else
        return false;
}


int main( int argc, const char** argv )
{
    CvCapture* capture = 0;
    Mat frame, frameCopy, rgbImage, depthImg;
    const String scaleOpt = "--scale=";
    size_t scaleOptLen = scaleOpt.length();
    const String faceCascadeOpt = "--face-cascade=";
    size_t faceCascadeOptLen = faceCascadeOpt.length();
    const String upperbodyCascadeOpt = "--body-cascade=";
    size_t upperbodyCascadeOptLen = upperbodyCascadeOpt.length();
    const String rgbImgOpt = "--rgbimg=";
    size_t rgbImgOptLen = rgbImgOpt.length();
    const String depthImgOpt = "--depthimg=";
    size_t depthImgOptLen = depthImgOpt.length();
    String rgbName (""), depthName ("");

    String faceCascadeName, upperbodyCascadeName;
    CascadeClassifier faceCascade, upperbodyCascade;
    double scale = 1;

    for (int i = 1; i < argc; i++)
    {
        cout << "Processing " << i << ": " << argv[i] << endl;
        // compare cascadeOpt (starting from 0, length cascadeOptLen) with argv[i] (length cascadeOptLen)
        if (faceCascadeOpt.compare(0, faceCascadeOptLen, argv[i], faceCascadeOptLen) == 0)
            faceCascadeName.assign(argv[i] + faceCascadeOptLen);
        else if (upperbodyCascadeOpt.compare(0, upperbodyCascadeOptLen, argv[i], upperbodyCascadeOptLen) == 0)
            upperbodyCascadeName.assign(argv[i] + upperbodyCascadeOptLen);
        else if (scaleOpt.compare(0, scaleOptLen, argv[i], scaleOptLen) == 0)
        {
            if (!sscanf(argv[i] + scaleOpt.length(), "%lf", &scale) || scale < 1)
                scale = 1;
        }
        else if (rgbImgOpt.compare(0, rgbImgOptLen, argv[i], rgbImgOptLen) == 0)
            rgbName.assign(argv[i] + rgbImgOptLen);
        else if (depthImgOpt.compare(0, depthImgOptLen, argv[i], depthImgOptLen) == 0)
            depthName.assign(argv[i] + depthImgOptLen);
        else if (argv[i][0] == '-')
        {
            cerr << "WARNING: Unknown option %s" << argv[i] << endl;
            return -1;
        }
    }

    if (!faceCascade.load(faceCascadeName))
    {
        cerr << "ERROR: Could not load face cascade" << endl;
        return -1;
    }
    cout << upperbodyCascadeName << endl;
    if (!upperbodyCascade.load(upperbodyCascadeName))
    {
        cerr << "ERROR: Could not load upperbody cascade" << endl;
        return -1;
    }

    if (rgbName.empty() || depthName.empty())
    {
        cerr << "No input for RGB image or depth image." << endl;
        return -1;
    }
    else
    {
        rgbImage = imread(rgbName, 1);
        depthImg = imread(depthName, 1);
        if (rgbImage.empty() || depthImg.empty())
        {
            cerr << "Cannot open the RGB image or depth image." << endl;
            return -1;
        }
    }


    cvNamedWindow("result", 1);
    detectAndDraw(rgbImage, depthImg, faceCascade, upperbodyCascade, scale);
    waitKey(0);
    cvDestroyWindow("result");

    return 0;
}


void detectAndDraw( Mat& img, Mat& depthimg,
                   CascadeClassifier& faceCascade, CascadeClassifier& upperbodyCascade,
                   double scale)
{
    int i = 0;
    double t = 0;
    vector<Rect> faces, bodies;
    const static Scalar colors[] =  {CV_RGB(255,0,0), CV_RGB(0,255,0), 
        CV_RGB(0,0,255), CV_RGB(0,128,255), CV_RGB(0,255,255), 
        CV_RGB(255,128,0), CV_RGB(255,255,0), CV_RGB(255,0,255)} ;
    Mat gray, smallImg(cvRound (img.rows/scale), cvRound(img.cols/scale), CV_8UC1);

    cvtColor(img, gray, CV_BGR2GRAY);
    resize(gray, smallImg, smallImg.size(), 0, 0, INTER_LINEAR);
    equalizeHist(smallImg, smallImg);

    t = (double)cvGetTickCount();
    faceCascade.detectMultiScale(smallImg, faces, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, Size(30, 30));
    upperbodyCascade.detectMultiScale(smallImg, bodies, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, Size(40, 40));
    t = (double)cvGetTickCount() - t;

    printf("detection time = %g ms\n", t/((double)cvGetTickFrequency()*1000.));
    i = faces.size() - 1;
    for (vector<Rect>::const_iterator r = faces.end() - 1; r >= faces.begin(); r--)
    {
        int depth;
        if (depthimg.type() == CV_8UC3)
            depth = depthimg.at<Vec3b>(r->y + r->height * 0.5, r->x + r->width * 0.5, 0)[0];
        else
        {
            cerr << "Image type is not covered" << endl;
            abort();
        }

        bool flag = CheckFaceDepth(depth, r->width);
//        CvPoint pt1 = cvPoint(r->x, r->y);
//        CvPoint pt2 = cvPoint(r->x + r->width, r->y + r->height);
        if (!flag)
//        {
//            rectangle(img, pt1, pt2, CV_RGB(200, 0, 0), 1, 8, 0);
            faces.erase(faces.begin() + i);
//        }
//        else
//            rectangle(img, pt1, pt2, CV_RGB(200, 0, 0), 3, 8, 0);
        i--;
    }

//    for (vector<Rect>::const_iterator r = bodies.begin(); r != bodies.end(); r++)
    i = bodies.size() - 1;
    for (vector<Rect>::const_iterator r = bodies.end() - 1; r >= bodies.begin(); r--)
    {
//        cout << "Face center: (" << r->x + r->width / 2 << ","
//            << r->y + r->height / 2 << ")    ";
//        cout << "Face size: (" << r->width << "," << r->height << ")    ";

        int depth;
        if (depthimg.type() == CV_8UC3)
            depth = depthimg.at<Vec3b>(r->y + r->height, r->x + r->width * 0.5, 0)[0];
        else
        {
            cerr << "Image type is not covered" << endl;
            abort();
        }

        bool flag = CheckUpperbodyDepth(depth, r->width);
//        CvPoint pt1 = cvPoint(r->x, r->y);
//        CvPoint pt2 = cvPoint(r->x + r->width, r->y + r->height);
        if (!flag)
//        {
//            rectangle(img, pt1, pt2, CV_RGB(0, 200, 0), 1, 8, 0);
            bodies.erase(bodies.begin() + i);
//        }
//        else
//            rectangle(img, pt1, pt2, CV_RGB(0, 255, 0), 3, 8, 0);
        i--;
    }

    vector<Rect> finalFaces;  // final results are in "bodies" and "finalFaces"
    for (int r = bodies.size() - 1; r >= 0; r--)
    {
        int s = 0;
        for (s = 0; s < faces.size(); s++)
        {
            if ((faces[s].x > bodies[r].x + bodies[r].width / 8)
                  && (faces[s].x + faces[s].width < bodies[r].x + bodies[r].width * 7 / 8)
                  && (faces[s].y + faces[s].height < bodies[r].y + bodies[r].height * 7 / 8))
            {
                finalFaces.push_back(faces[s]);
                break;
            }
        }
        if (s == faces.size())
            bodies.erase(bodies.begin() + r);
    }

    assert(bodies.size() == finalFaces.size());
    if (bodies.size() == 0)
        cout << "NO HUMAN" << endl;

    for (int r = 0; r < bodies.size(); r++)
    {
        CvPoint pt1 = cvPoint(bodies[r].x, bodies[r].y);
        CvPoint pt2 = cvPoint(bodies[r].x + bodies[r].width, bodies[r].y + bodies[r].height);
        rectangle(img, pt1, pt2, CV_RGB(0, 255, 0), 3, 8, 0);
        pt1 = cvPoint(finalFaces[r].x, finalFaces[r].y);
        pt2 = cvPoint(finalFaces[r].x + finalFaces[r].width, 
              finalFaces[r].y + finalFaces[r].height);
        rectangle(img, pt1, pt2, CV_RGB(255, 0, 0), 3, 8, 0);

        HandDetect(img, depthimg, finalFaces[r], bodies[r]);
    }

//    cv::imshow( "result", img );
}


// normalize the RGB image and depth image such that 
void Normalize(Mat oriRgbImg, Mat oriDepthImg, Rect oriFace, Rect oriBody, 
               Mat &rgbImg, Mat &depthImg, Rect &face, Rect &body)
{
    // extracting sub-matrix
    int left = max(oriBody.x - oriBody.width, 0);
    int right = min(oriBody.x + oriBody.width * 2, oriRgbImg.cols);
    int top = max(oriBody.y - oriBody.height / 2, 0);
    int bottom = min(oriBody.y + oriBody.height * 5 / 2, oriRgbImg.rows);

    Mat tmpRgbImg(bottom - top, right - left, oriRgbImg.type());
    tmpRgbImg = oriRgbImg(Range(top, bottom), Range(left, right));
    Mat tmpDepthImg(bottom - top, right - left, oriDepthImg.type());
    tmpDepthImg = oriDepthImg(Range(top, bottom), Range(left, right));

    Rect tmpFace = Rect(oriFace.x - left, oriFace.y - top, oriFace.width, oriFace.height);
    Rect tmpBody = Rect(oriBody.x - left, oriBody.y - top, oriBody.width, oriBody.height);

    // re-scaling
    double scale = 125 / (double)tmpBody.width;
    rgbImg.create(cvRound(tmpRgbImg.rows * scale), cvRound(tmpRgbImg.cols * scale),
          tmpRgbImg.type());
    resize(tmpRgbImg, rgbImg, rgbImg.size());
    depthImg.create(cvRound(tmpDepthImg.rows * scale), cvRound(tmpDepthImg.cols * scale),
          tmpDepthImg.type());
    resize(tmpDepthImg, depthImg, depthImg.size());

    face.x = tmpFace.x * scale;
    face.y = tmpFace.y * scale;
    face.width = tmpFace.width * scale;
    face.height = tmpFace.height * scale;
    body.x = tmpBody.x * scale;
    body.y = tmpBody.y * scale;
    body.width = tmpBody.width * scale;
    body.height = tmpBody.height * scale;
}


/*
void ConvexDegreeFeature(Mat depthImg, Mat &cdfImg, int innerR, int outerR, int offset)
{
    int imgHeight = depthImg.rows;
    int imgWidth = depthImg.cols;
    int outDesire, inDesire, outTotal, inTotal, value;
    float Nout, Nin, maxCDF = 0;

    for (int h = outerR; h < imgHeight - outerR; h++)
    {
        for (int w = outerR; w < imgWidth - outerR; w++)
        {
            // get the minimum depth value in 'innerR'
            int threshold = 255;
            for (int y = -innerR; y <= innerR; y++)
            {
                for (int x = -innerR; x <= innerR; x++)
                {
                    value = depthImg.at<Vec3b>(h + y, w + x)[0];
                    if (value < 15)  // the points with depth value smaller than 15 is meaningless
                        continue;
                    if (threshold > value)
                        threshold = value;
                }
            }
            threshold += offset;

            // compute Nout and Nin
            outDesire = 0;
            inDesire = 0;
            outTotal = 0;
            inTotal = 0;
            for (int y = -outerR; y <= outerR; y++)
            {
                for (int x = -outerR; x <= outerR; x++)
                {
                    value = depthImg.at<Vec3b>(h + y, w + x)[0];
                    if (value < 15)
                        continue;
                    if ((y >= -innerR) && (y <= innerR) && (x >= -innerR) && (x <= innerR))
                    {
                        inTotal++;
                        if (value < threshold)
                            inDesire++;
                    }
                    else
                    {
                        outTotal++;
                        if (value > threshold)
                            outDesire++;
                    }
                }
            }

            if ((inTotal == 0) || (outTotal == 0) || ((inDesire == 0) && (outDesire == 0)))
                continue;
            Nout = (float)outDesire / (float)outTotal;
            Nin = (float)inDesire / (float)inTotal;
//            cout << "(" << Nin << "," << Nout << ")  ";
            cdfImg.at<float>(h, w) = exp(-1 / (Nout + Nin));
//            cout << cdfImg.at<float>(h, w) << "  ";
            if (exp(-1 / (Nout + Nin)) > maxCDF)
                maxCDF = exp(-1 / (Nout + Nin));
        }
    }

    float scale = 255 / maxCDF;
}
*/


void ConvexDegreeFeature(Mat depthImg, Mat &cdfImg, int innerR, int outerR)
{
    #define CDF_BIN_NUM 32
    #define CDF_BIN_SIZE 8

    int imgHeight = depthImg.rows;
    int imgWidth = depthImg.cols;
    float outHist[CDF_BIN_NUM], inHist[CDF_BIN_NUM], value, distance;
    float *outHistPTR, *inHistPTR;
    float outSum, inSum, maxCDF = 0;

    for (int h = outerR; h < imgHeight - outerR; h++)
    {
        for (int w = outerR; w < imgWidth - outerR; w++)
        {
            memset(outHist, 0, CDF_BIN_NUM * sizeof(int));
            memset(inHist, 0, CDF_BIN_NUM * sizeof(int));

            for (int y = -outerR; y <= outerR; y++)
            {
                for (int x = -outerR; x <= outerR; x++)
                {
                    value = depthImg.at<Vec3b>(h + y, w + x)[0];
                    if (value < 15)
                        continue;
                    if ((y >= -innerR) && (y <= innerR) && (x >= -innerR) && (x <= innerR))
                        inHist[(int)(value / CDF_BIN_SIZE)] += 1;
                    else
                        outHist[(int)(value / CDF_BIN_SIZE)] += 1;
                }
            }

            // normalize the distributions
            outSum = 0;
            inSum = 0;
            outHistPTR = outHist;
            inHistPTR = inHist;
            for (int i = 0; i < CDF_BIN_NUM; i++)
            {
                outSum += *outHistPTR;
                outHistPTR++;
                inSum += *inHistPTR;
                inHistPTR++;
            }
            if ((outSum == 0) || (inSum == 0))
                continue;

            outHistPTR = outHist;
            inHistPTR = inHist;
            for (int i = 0; i < CDF_BIN_NUM; i++)
            {
                *outHistPTR /= outSum;
                outHistPTR++;
                *inHistPTR /= inSum;
                inHistPTR++;
            }

            // compute the chi-square distance
            outHistPTR = outHist;
            inHistPTR = inHist;
            distance = 0;
            for (int i = 0; i < CDF_BIN_NUM; i++)
            {
                if ((*outHistPTR == 0) && (*inHistPTR == 0))
                {
                    outHistPTR++;
                    inHistPTR++;
                    continue;
                }
                distance += (*outHistPTR - *inHistPTR) * (*outHistPTR - *inHistPTR) 
                      / (*outHistPTR + *inHistPTR);
                outHistPTR++;
                inHistPTR++;
            }

            cdfImg.at<float>(h, w) = distance;
            if (distance > maxCDF)
                maxCDF = distance;
        }
    }
}


void DepthFilter(Mat &cdfImg, Mat depthImg, Rect face, Rect body, int tolerance)
{
    int faceDepth = depthImg.at<Vec3b>(face.y + face.height / 2, face.x + face.width / 2)[0];
    int bodyDepth = depthImg.at<Vec3b>(body.y + body.height, body.x + body.width / 2)[0];
    int imgHeight = depthImg.rows;
    int imgWidth = depthImg.cols;
    int value;

    for (int h = 0; h < imgHeight; h++)
    {
        for (int w = 0; w < imgWidth; w++)
        {
            value = depthImg.at<Vec3b>(h, w)[0];
            if (((value < faceDepth - tolerance) || (value > faceDepth + tolerance))
                  && ((value < bodyDepth - tolerance) || (value > bodyDepth + tolerance)))
                cdfImg.at<float>(h, w) = 0;
        }
    }
}


void ColorFilter(Mat &cdfImg, Mat rgbImg, Rect face, int tolerance)
{
    #define CLR_BIN_SIZE 32
    #define CLR_SBIN_NUM 8
    #define CLR_BIN_NUM 8 * 8 * 8

    int xEnd = face.x + face.width * 3 / 4;
    int yEnd = face.y + face.height * 3 / 4;
    int hist[CLR_BIN_NUM], valueR, valueG, valueB, histID;
    memset(hist, 0, CLR_BIN_NUM * sizeof(int));
    for (int y = face.y + face.height / 4; y < yEnd; y++)
    {
        for (int x = face.x + face.width / 4; x < xEnd; x++)
        {
            valueR = rgbImg.at<Vec3b>(y, x)[0];
            valueG = rgbImg.at<Vec3b>(y, x)[1];
            valueB = rgbImg.at<Vec3b>(y, x)[2];
            histID = valueR / CLR_BIN_SIZE * CLR_SBIN_NUM * CLR_SBIN_NUM + valueG 
                  / CLR_BIN_SIZE * CLR_SBIN_NUM + valueB / CLR_BIN_SIZE;
            hist[histID] += 1;
        }
    }

    // the max component in the RGB hist
    int maxHist = 0, maxID;
    int *histPTR = hist;
    for (int i = 0; i < CLR_BIN_NUM; i++)
    {
        if (maxHist < *histPTR)
        {
            maxID = i;
            maxHist = *histPTR;
        }
        histPTR++;
    }
    int maxRMin = (int)(maxID / (CLR_SBIN_NUM * CLR_SBIN_NUM)) * CLR_BIN_SIZE;
    int maxRMax = maxRMin + CLR_BIN_SIZE;
    int maxGMin = (int)((maxID % (CLR_SBIN_NUM * CLR_SBIN_NUM)) / CLR_SBIN_NUM) * CLR_BIN_SIZE;
    int maxGMax = maxGMin + CLR_BIN_SIZE;
    int maxBMin = (maxID % CLR_SBIN_NUM) * CLR_BIN_SIZE;
    int maxBMax = maxBMin + CLR_BIN_SIZE;
    
    int imgHeight = rgbImg.rows;
    int imgWidth = rgbImg.cols;
    for (int h = 0; h < imgHeight; h++)
    {
        for (int w = 0; w < imgWidth; w++)
        {
            if (cdfImg.at<float>(h, w) == 0)
                continue;

            valueR = rgbImg.at<Vec3b>(h, w)[0];
            valueG = rgbImg.at<Vec3b>(h, w)[1];
            valueB = rgbImg.at<Vec3b>(h, w)[2];
            if ((valueR < maxRMin - tolerance) || (valueR > maxRMax + tolerance)
                  || (valueG < maxGMin - tolerance) || (valueG > maxGMax + tolerance)
                  || (valueB < maxBMin - tolerance) || (valueB > maxBMax + tolerance))
                cdfImg.at<float>(h, w) = 0;
        }
    }
}


void CDFFilter(Mat &cdfImg, float tolerance)
{
    int imgHeight = cdfImg.rows;
    int imgWidth = cdfImg.cols;
    for (int h = 0; h < imgHeight; h++)
    {
        for (int w = 0; w < imgWidth; w++)
        {
            if (cdfImg.at<float>(h, w) < tolerance)
                cdfImg.at<float>(h, w) = 0;
        }
    }
}


void AppFilter(Mat &cdfImg, Rect face, int &posID)
{
    int left = face.x;
    int right = face.x + face.width;
    int top = face.y;
    int bottom = face.y + face.height * 3;
    for (int y = top; y < bottom; y++)
    {
        for (int x = left; x < right; x++)
            cdfImg.at<float>(y, x) = 0;
    }

    int imgHeight = cdfImg.rows;
    int imgWidth = cdfImg.cols;
    left = max(0, face.x - face.width / 4);
    right = min(imgWidth, face.x + face.width * 4 / 5);
    top = max(0, face.y);
    bottom = min(imgHeight, face.y + face.height * 4 / 3);
    int int1 = face.y + face.height / 3;
    int int2 = face.y + face.height * 2 / 3;
    int int3 = face.y + face.height;
    int posNum[4] = {0, 0, 0, 0};

    for (int h = 0; h < imgHeight; h++)
    {
        for (int w = 0; w < imgWidth; w++)
        {
            if ((w > left) && (w < right) && (h > top) && (h < bottom))
            {
                if (cdfImg.at<float>(h, w) > 0.001)
                {
                    cdfImg.at<float>(h, w) = 0.99;
                    if (h < int1)
                        posNum[0]++;  // pointing head
                    else if (h < int2)
                        posNum[1]++;  // pointing eye
                    else if (h < int3)
                        posNum[2]++;  // pointing cheek
                    else
                        posNum[3]++;  // pointing chin
                }
                continue;
            }
            cdfImg.at<float>(h, w) = 0;
        }
    }

    if ((posNum[1] > posNum[0]) && (posNum[1] > posNum[2]) && (posNum[1] > posNum[3]))
        posID = 0;  // eye
    else if ((posNum[3] > posNum[0]) && (posNum[3] > posNum[1]) && (posNum[3] > posNum[2]))
        posID = 1;  // chin
    else
        posID = -1;
    cout << posNum[0] << " " << posNum[1] << " " << posNum[2] << " " << posNum[3] << endl;
}


void HandDetect(Mat oriRgbImg, Mat oriDepthImg, Rect oriFace, Rect oriBody)
{
    double scale = 125 / (double)oriBody.width;  // the normalized width of upper body is 125
    Mat rgbImg, depthImg;  // normalized images
    Rect face, body;  // normalized face and upper body
    Normalize(oriRgbImg, oriDepthImg, oriFace, oriBody, rgbImg, depthImg, face, body);

    // candidate hand regions: Convex Degree Feature feature
    Mat cdfImg = Mat::zeros(depthImg.rows, depthImg.cols, CV_32F);
    ConvexDegreeFeature(depthImg, cdfImg, 10, 15);
//    cv::imshow("cdf", cdfImg);

    // filter based on depth
    int depthTolerance = 4;
    DepthFilter(cdfImg, depthImg, face, body, depthTolerance);
//    cv::imshow("cdf2", cdfImg);

    // filter based on color information
    int colorTolerance = 32;
    ColorFilter(cdfImg, rgbImg, face, colorTolerance);
//    cv::imshow("cdf3", cdfImg);
    
    // filter based on CDF value
    float cdfTolerance = 0.1;
    CDFFilter(cdfImg, cdfTolerance);
//    cv::imshow("cdf4", cdfImg);

    // filter based on application based prior
    int posID;
    AppFilter(cdfImg, face, posID);
//    cv::imshow("cdf5", cdfImg);
    if (posID == 0)
        cout << endl << "EYE" << endl;
    else if (posID == 1)
        cout << endl << "CHIN" << endl;
    else
        cout << endl << "NO ACTION" << endl;

    CvPoint pt1 = cvPoint(body.x, body.y);
    CvPoint pt2 = cvPoint(body.x + body.width, body.y + body.height);
    rectangle(rgbImg, pt1, pt2, CV_RGB(0, 255, 0), 3, 8, 0);
    rectangle(depthImg, pt1, pt2, CV_RGB(0, 255, 0), 3, 8, 0);
    pt1 = cvPoint(face.x, face.y);
    pt2 = cvPoint(face.x + face.width, face.y + face.height);
    rectangle(rgbImg, pt1, pt2, CV_RGB(255, 0, 0), 3, 8, 0);
    rectangle(depthImg, pt1, pt2, CV_RGB(255, 0, 0), 3, 8, 0);
    cv::imshow("rgb", rgbImg);
    cv::imshow("depth", depthImg);
}

















































